
#include "CQuaternion.h"



CQuaternion::CQuaternion()
{ /* empty ctor */ }


CQuaternion::CQuaternion( const fixed* q )
{
	ASSERT( q ); 
	m_i = q[0];
	m_j = q[1];
	m_k = q[2];
	m_w = q[3];
}

 
CQuaternion::CQuaternion( fixed i, fixed j, fixed k, fixed w )
{ 
	m_i = i;
	m_j = j;
	m_k = k;
	m_w = w;
}

CQuaternion::CQuaternion( fixed scalar, const CVector3d& vector )
{
	m_i = vector.m_i;
	m_j = vector.m_j;
	m_k = vector.m_k;
	m_w = scalar;
}

CQuaternion::CQuaternion( const CMatrix4dh& m )
{ 
	ASSERT( FALSE ); // Not yet implemented
} 

CQuaternion::operator CMatrix3d() const
{
	// main diagonal
	fixed m00 = CMathFixed::One - 
		( ( CMathFixed::Mul(m_j,m_j) + CMathFixed::Mul(m_k,m_k) ) << 1 );
	fixed m11 = CMathFixed::One -
		( ( CMathFixed::Mul(m_i,m_i) + CMathFixed::Mul(m_k,m_k) ) << 1 );
	fixed m22 = CMathFixed::One -
		( ( CMathFixed::Mul(m_i,m_i) + CMathFixed::Mul(m_j,m_j) ) << 1 );

	// lower triangle
	fixed m01 = ( CMathFixed::Mul(m_i,m_j) - CMathFixed::Mul(m_w,m_k) ) << 1;
	fixed m02 = ( CMathFixed::Mul(m_i,m_k) + CMathFixed::Mul(m_w,m_j) ) << 1;
	fixed m12 = ( CMathFixed::Mul(m_j,m_k) - CMathFixed::Mul(m_w,m_i) ) << 1;

	// upper triangle
	fixed m10 = ( CMathFixed::Mul(m_i,m_j) + CMathFixed::Mul(m_w,m_k) ) << 1;
	fixed m20 = ( CMathFixed::Mul(m_i,m_k) - CMathFixed::Mul(m_w,m_j) ) << 1;
	fixed m21 = ( CMathFixed::Mul(m_j,m_k) + CMathFixed::Mul(m_w,m_i) ) << 1;

	// assemble as required (note the spiffy column-major indices)
	return CMatrix3d( m00, m01, m02,
					  m10, m11, m12,
					  m20, m21, m22 );
}

CQuaternion& CQuaternion::Normalize()
{
	return *this/=Length();
}

CQuaternion CQuaternion::Normalized() const
{
	return *this/Length();
}

CQuaternion& CQuaternion::operator*=( CQuaternion const& other )
{
	m_w = CMathFixed::Mul( m_w, other.m_w ) - CMathFixed::Mul( m_i, other.m_i ) - CMathFixed::Mul( m_j, other.m_j ) - CMathFixed::Mul( m_k, other.m_k );

	m_i = CMathFixed::Mul( m_j, other.m_k ) - CMathFixed::Mul( m_k, other.m_j ) + CMathFixed::Mul( m_w, other.m_i ) + CMathFixed::Mul( m_i, other.m_w );
	m_j = CMathFixed::Mul( m_k, other.m_i ) - CMathFixed::Mul( m_i, other.m_k ) + CMathFixed::Mul( m_w, other.m_j ) + CMathFixed::Mul( m_j, other.m_w );
	m_k = CMathFixed::Mul( m_i, other.m_j ) - CMathFixed::Mul( m_j, other.m_i ) + CMathFixed::Mul( m_w, other.m_k ) + CMathFixed::Mul( m_k, other.m_w );

	return *this;
}

//! spherical linear interpolation
CQuaternion slerp(const CQuaternion &q1, const CQuaternion &q2, fixed u) 
{
	CQuaternion q3;
	fixed d = dot( q1, q2 );

	/*	dot = cos(theta)
		if (dot < 0), q1 and q2 are more than 90 degrees apart,
		so we can invert one to reduce spinning	*/
	if (d < 0) {
		d = -d;
		q3 = -q2;
	} else {
		q3 = q2;
	}
	
	if ( d < CMathFixed_FloatToFixed(0.95f) ) {
		fixed angle = CMathFixed::ACos( d );
		return ( q1 * CMathFixed::Sin( CMathFixed::Mul( angle, CMathFixed::One - u ) ) ) + 
			   ( ( q3 * CMathFixed::Sin( CMathFixed::Mul( angle, u ) ) ) / CMathFixed::Sin( angle ) );
	} else {
		// if the angle is small, use linear interpolation								
		return lerp( q1, q3, u );		
	}
}

CQuaternion slerpNoInvert(const CQuaternion &q1, const CQuaternion &q2, fixed u)
{
	fixed d = dot( q1, q2 );

	if ( CMathFixed::Abs( d ) < CMathFixed_FloatToFixed(0.95f) )
	{
		fixed angle = CMathFixed::ACos( d );
		return ( q1 * CMathFixed::Sin( CMathFixed::Mul( angle, CMathFixed::One - u ) ) ) + 
			   ( ( q2 * CMathFixed::Sin( CMathFixed::Mul( angle, u ) ) ) / CMathFixed::Sin( angle ) );
	} else {
		// if the angle is small, use linear interpolation								
		return lerp( q1, q2, u );
	}
}

void CQuaternion::to_axis_angle(CVector3d &axis, fixed &angle) const
{
	// half the angle -- we'll rectify in a bit
	angle = CMathFixed::ACos( m_w );

	// pre-compute to save time
	fixed sinf_theta_inv = CMathFixed::Inv( CMathFixed::Sin( angle ) );

	// now the vector
	axis[0] = CMathFixed::Mul( m_i, sinf_theta_inv );
	axis[1] = CMathFixed::Mul( m_j, sinf_theta_inv );
	axis[2] = CMathFixed::Mul( m_k, sinf_theta_inv );

	// now the angle
	angle <<= 1;
}

CVector3d CQuaternion::to_euler_angles() const
{
	fixed sqw = CMathFixed::Mul(m_w,m_w);    
	fixed sqx = CMathFixed::Mul(m_i,m_i);    
	fixed sqy = CMathFixed::Mul(m_j,m_j);    
	fixed sqz = CMathFixed::Mul(m_k,m_k);    

	CVector3d euler;
	euler[0] = CMathFixed::ATan( CMathFixed::Div( ( CMathFixed::Mul(m_i,m_j) + CMathFixed::Mul(m_k,m_w) ) << 1, sqx - sqy - sqz + sqw ) );    		
	euler[1] = CMathFixed::ASin( -( ( CMathFixed::Mul(m_i,m_k) - CMathFixed::Mul(m_j,m_w) ) << 1 ) );
	euler[2] = CMathFixed::ATan( CMathFixed::Div( ( CMathFixed::Mul(m_j,m_k) + CMathFixed::Mul(m_i,m_w) ) << 1, -sqx - sqy + sqz + sqw ) );    
	return euler;
}

CQuaternion CQuaternion::log() const
{
	fixed a = CMathFixed::ACos( m_w );
	fixed sina =  CMathFixed::Sin( a );

	if (sina > 0) {
		fixed sinc_a = CMathFixed::Div( a, sina );
		return CQuaternion( CMathFixed::Mul( m_i, sinc_a ),
							CMathFixed::Mul( m_j, sinc_a ),
							CMathFixed::Mul( m_k, sinc_a ), 0 );
	}

	return CQuaternion( 0, 0, 0, 0 );
}

CQuaternion CQuaternion::exp() const
{
	fixed a = CMathFixed::Mul( m_i, m_i ) + CMathFixed::Mul( m_j, m_j ) + CMathFixed::Mul( m_k, m_k );
	fixed sina = CMathFixed::Sin( a );
	fixed cosa = CMathFixed::Cos( a );

	if (a > 0) {
		fixed sinc_ai = CMathFixed::Div( sina, a );
		return CQuaternion( CMathFixed::Mul( m_i, sinc_ai ),
							CMathFixed::Mul( m_j, sinc_ai ),
							CMathFixed::Mul( m_k, sinc_ai ), cosa );
	}

	return CQuaternion( 0, 0, 0, cosa );
}
